#pragma once

#include "i_brief.h"

namespace idl
{
  class InterestPointFastHessian : public InterestPointBase<float, 64>
  {
  public:
     InterestPointFastHessian();
    ~InterestPointFastHessian();
    inline void normalize(); //normalize the descriptor vector to unit length 
  };

  class FastHessianResponseLayer : public InterestPointResponseLayer<float>
  {
  public:
    typedef InterestPointResponseLayer<float> Base;
    FastHessianResponseLayer() : laplacian(NULL), step(0), filter(0), lobe(0), radius(0), area(0){};
    FastHessianResponseLayer(int width, int height, int step, int filter);
    ~FastHessianResponseLayer(){ i_free2<unsigned char>(laplacian); }
    int step/*sampling step*/, filter/*filter size length*/, lobe/*length of dark lobe->1/3 of the filter size length*/, radius/*filter raidus*/, area/*filter area*/;
    int bound[4]; //begin_x, end_x, begin_y, end_y;
    inline unsigned char get_laplacian(int x, int y) const;
    inline void set_laplacian(int x, int y, unsigned char val);
  private:
    unsigned char **laplacian;
  };

  class FastHessianDetector/*Interest points detection algorithm proposed in SURF*/
  {
  public:
    static const int MAX_NR_OCTAVES = 5; //do not change!
    static const int MAX_NR_RESPONSE_LAYERS = 12; //do not change!
    FastHessianDetector() :_width(0), _height(0), _octaves(0), _nr_layers(0){};
    FastHessianDetector(
      const int width,
      const int height,
      const int octaves = MAX_NR_OCTAVES,
      const int init_sample = I_DEFAULT_FEATURE_SAMPLE);
                        
    ~FastHessianDetector();

    int detect(const float *const *integral, std::vector<InterestPointFastHessian> &ipts, const float thres = I_DEFAULT_FEATURE_HESSIAN_RESPONSE_THRESHOLD);
    int detect(const float *const *integral, std::vector<InterestPointBrief128> &ipts, const float thres = I_DEFAULT_FEATURE_HESSIAN_RESPONSE_THRESHOLD);
    int detect(const float *const *integral, std::vector<InterestPointBrief256> &ipts, const float thres = I_DEFAULT_FEATURE_HESSIAN_RESPONSE_THRESHOLD);
    int detect(const float *const *integral, std::vector<InterestPointBrief512> &ipts, const float thres = I_DEFAULT_FEATURE_HESSIAN_RESPONSE_THRESHOLD);

  private:
    void buildHessianResponseLayer(const float *const *integral, FastHessianResponseLayer* layer);
    
    int extratExtrema(const FastHessianResponseLayer *up, const FastHessianResponseLayer *ct, const FastHessianResponseLayer *dn, 
                      std::vector<InterestPointFastHessian> &ipts);

    int extratExtrema(const FastHessianResponseLayer *up, const FastHessianResponseLayer *ct, const FastHessianResponseLayer *dn,
                      std::vector<InterestPointBrief128> &ipts);

    int extratExtrema(const FastHessianResponseLayer *up, const FastHessianResponseLayer *ct, const FastHessianResponseLayer *dn,
                      std::vector<InterestPointBrief256> &ipts);

    int extratExtrema(const FastHessianResponseLayer *up, const FastHessianResponseLayer *ct, const FastHessianResponseLayer *dn,
                      std::vector<InterestPointBrief512> &ipts);

    //extratExtrema funcs that smaple local extrema ar differebr octaves SURF
    int extratExtrema_1(const FastHessianResponseLayer &up, const FastHessianResponseLayer &ct, const FastHessianResponseLayer &dn,
                        std::vector<InterestPointFastHessian> &ipts);
    int extratExtrema_2(const FastHessianResponseLayer &up, const FastHessianResponseLayer &ct, const FastHessianResponseLayer &dn,
                        std::vector<InterestPointFastHessian> &ipts);
    int extratExtrema_3(const FastHessianResponseLayer &up, const FastHessianResponseLayer &ct, const FastHessianResponseLayer &dn,
                        std::vector<InterestPointFastHessian> &ipts);
    int extratExtrema_4(const FastHessianResponseLayer &up, const FastHessianResponseLayer &ct, const FastHessianResponseLayer &dn,
                        std::vector<InterestPointFastHessian> &ipts);       

    //extratExtrema funcs that smaple local extrema ar differebr octaves BRIEF-128
    int extratExtrema_1(const FastHessianResponseLayer &up, const FastHessianResponseLayer &ct, const FastHessianResponseLayer &dn,
                        std::vector<InterestPointBrief128> &ipts);
    int extratExtrema_2(const FastHessianResponseLayer &up, const FastHessianResponseLayer &ct, const FastHessianResponseLayer &dn,
                        std::vector<InterestPointBrief128> &ipts);
    int extratExtrema_3(const FastHessianResponseLayer &up, const FastHessianResponseLayer &ct, const FastHessianResponseLayer &dn,
                        std::vector<InterestPointBrief128> &ipts);
    int extratExtrema_4(const FastHessianResponseLayer &up, const FastHessianResponseLayer &ct, const FastHessianResponseLayer &dn,
                        std::vector<InterestPointBrief128> &ipts);

    //extratExtrema funcs that smaple local extrema ar differebr octaves BRIEF-256
    int extratExtrema_1(const FastHessianResponseLayer &up, const FastHessianResponseLayer &ct, const FastHessianResponseLayer &dn,
                        std::vector<InterestPointBrief256> &ipts);
    int extratExtrema_2(const FastHessianResponseLayer &up, const FastHessianResponseLayer &ct, const FastHessianResponseLayer &dn,
                        std::vector<InterestPointBrief256> &ipts);
    int extratExtrema_3(const FastHessianResponseLayer &up, const FastHessianResponseLayer &ct, const FastHessianResponseLayer &dn,
                        std::vector<InterestPointBrief256> &ipts);
    int extratExtrema_4(const FastHessianResponseLayer &up, const FastHessianResponseLayer &ct, const FastHessianResponseLayer &dn,
                        std::vector<InterestPointBrief256> &ipts);

    //extratExtrema funcs that smaple local extrema ar differebr octaves BRIEF-512
    int extratExtrema_1(const FastHessianResponseLayer &up, const FastHessianResponseLayer &ct, const FastHessianResponseLayer &dn,
                        std::vector<InterestPointBrief512> &ipts);
    int extratExtrema_2(const FastHessianResponseLayer &up, const FastHessianResponseLayer &ct, const FastHessianResponseLayer &dn,
                        std::vector<InterestPointBrief512> &ipts);
    int extratExtrema_3(const FastHessianResponseLayer &up, const FastHessianResponseLayer &ct, const FastHessianResponseLayer &dn,
                        std::vector<InterestPointBrief512> &ipts);
    int extratExtrema_4(const FastHessianResponseLayer &up, const FastHessianResponseLayer &ct, const FastHessianResponseLayer &dn,
                        std::vector<InterestPointBrief512> &ipts);

    int _width, _height, _octaves, _nr_layers;
    FastHessianResponseLayer *_layers[MAX_NR_RESPONSE_LAYERS]; //Hessian response map
    float _response_thres;
  };

  /*Feature descriptor implementation for SURF*/
  class InterestPointDescriptorSurf
  {
  public:
    InterestPointDescriptorSurf() :_width(0), _height(0), _dx24(NULL), _dy24(NULL), _dx24abs(NULL), _dy24abs(NULL){};
    InterestPointDescriptorSurf(const int width, const int height);
    ~InterestPointDescriptorSurf();
    //! Describe all features in the supplied vector
    void describe(const float *const *integral, std::vector<InterestPointFastHessian> &ipts, bool upright = true);

  private:
    inline void describeRotinva(const float *const *integral, InterestPointFastHessian &ipt);
    inline void describeUpright(const float *const *integral, InterestPointFastHessian &ipt);
   
    inline void genSummedImagesUpright(const float *const *integral, InterestPointFastHessian &ipt);
    inline void encodeDescriptor(InterestPointFastHessian &ipt);

    inline void genSubSummedImagesUpright1x1(const float *const *integral, int begin_x, int begin_y, int haarradius, int haarwidth, int row, int col);
    inline void genSummedImagesUpright1x1(const float *const *integral, int begin_x, int begin_y, int end_x, int end_y, int haarradius);

    inline void genSubSummedImagesUpright2x2(const float *const *integral, int begin_x, int begin_y, int haarradius, int haarwidth, int row, int col);
    inline void genSummedImagesUpright2x2(const float *const *integral, int begin_x, int begin_y, int end_x, int end_y, int haarradius);

    inline void genSubSummedImagesUpright3x3(const float *const *integral, int begin_x, int begin_y, int haarradius, int haarwidth, int row, int col);
    inline void genSummedImagesUpright3x3(const float *const *integral, int begin_x, int begin_y, int end_x, int end_y, int haarradius);

    inline void genSubSummedImagesUpright4x4(const float *const *integral, int begin_x, int begin_y, int haarradius, int haarwidth, int row, int col);
    inline void genSummedImagesUpright4x4(const float *const *integral, int begin_x, int begin_y, int end_x, int end_y, int haarradius);

    inline void genSubSummedImagesUpright5x5(const float *const *integral, int begin_x, int begin_y, int haarradius, int haarwidth, int row, int col);
    inline void genSummedImagesUpright5x5(const float *const *integral, int begin_x, int begin_y, int end_x, int end_y, int haarradius);

    inline void genSubSummedImagesUpright6x6(const float *const *integral, int begin_x, int begin_y, int haarradius, int haarwidth, int row, int col);
    inline void genSummedImagesUpright6x6(const float *const *integral, int begin_x, int begin_y, int end_x, int end_y, int haarradius);

    inline void genSubSummedImagesUprightnxn(const float *const *integral, int begin_x, int begin_y, int n, int nsqr, int haarradius, int haarwidth, int row, int col);
    inline void genSummedImagesUprightnxn(const float *const *integral, int begin_x, int begin_y, int end_x, int end_y, int haarradius, int scale);

    inline void encodeSubDescriptor00(InterestPointFastHessian &ipt);
    inline void encodeSubDescriptor01(InterestPointFastHessian &ipt);
    inline void encodeSubDescriptor02(InterestPointFastHessian &ipt);
    inline void encodeSubDescriptor03(InterestPointFastHessian &ipt);
    inline void encodeSubDescriptor10(InterestPointFastHessian &ipt);
    inline void encodeSubDescriptor11(InterestPointFastHessian &ipt);
    inline void encodeSubDescriptor12(InterestPointFastHessian &ipt);
    inline void encodeSubDescriptor13(InterestPointFastHessian &ipt);
    inline void encodeSubDescriptor20(InterestPointFastHessian &ipt);
    inline void encodeSubDescriptor21(InterestPointFastHessian &ipt);
    inline void encodeSubDescriptor22(InterestPointFastHessian &ipt);
    inline void encodeSubDescriptor23(InterestPointFastHessian &ipt);
    inline void encodeSubDescriptor30(InterestPointFastHessian &ipt);
    inline void encodeSubDescriptor31(InterestPointFastHessian &ipt);
    inline void encodeSubDescriptor32(InterestPointFastHessian &ipt);
    inline void encodeSubDescriptor33(InterestPointFastHessian &ipt);

    int _width, _height;
    float **_dx24, **_dy24, **_dx24abs, **_dy24abs; /*summed images, each has dimension 24x24 irrespective of the scale*/
    float _gaussian1[81], _gaussian2[16];
  };

#ifndef I_PLATFORM_NO_SSE_SUPPORT
  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  /*SURF Feature descriptor matching functions*/
  int i_match_interestpoints_fasthessian_sse(const std::vector<InterestPointFastHessian> &ipts_1,
                                                    const std::vector<InterestPointFastHessian> &ipts_2,
                                                    std::vector<Pair<int, int> >& correspondences,
                                                    double ratio_thre);
  
  int i_match_interestpoints_fasthessian_sse(const std::vector<InterestPointFastHessian> &ipts_1, 
                                                    const std::vector<InterestPointFastHessian> &ipts_2, 
                                                    std::vector<Pair<InterestPointFastHessian, InterestPointFastHessian> >& correspondences, 
                                                    double ratio_thre);

  int i_match_interestpoints_fasthessian_sse_uniqiue(const std::vector<InterestPointFastHessian> &ipts_1,
                                                     const std::vector<InterestPointFastHessian> &ipts_2,
                                                     std::vector<Pair<int, int> >& correspondences,
                                                     double ratio_thre);

  int i_match_interestpoints_fasthessian_sse_uniqiue(const std::vector<InterestPointFastHessian> &ipts_1,
                                                     const std::vector<InterestPointFastHessian> &ipts_2,
                                                     std::vector<Pair<InterestPointFastHessian, InterestPointFastHessian> >& correspondences,
                                                     double ratio_thre);

  int i_match_interestpoints_fasthessian(const std::vector<InterestPointFastHessian> &ipts_1,
                                         const std::vector<InterestPointFastHessian> &ipts_2,
                                         std::vector<Pair<int, int> >& correspondences,
                                         double ratio_thre = 0.7,
                                         int force_one_to_one = 0,
                                         int force_geometry = 0,
                                         double epi_dist_thre = 1.4142135623730950488016887242097, /*sqrt(2)*/
                                         double *fundamental = NULL);

  int i_match_interestpoints_fasthessian(const std::vector<InterestPointFastHessian> &ipts_1,
                                         const std::vector<InterestPointFastHessian> &ipts_2,
                                         std::vector<Pair<InterestPointFastHessian, InterestPointFastHessian> >& correspondences,
                                         double ratio_thre = 0.7,
                                         int force_one_to_one = 0,
                                         int force_geometry = 0,
                                         double epi_dist_thre = 1.4142135623730950488016887242097, /*sqrt(2)*/
                                         double *fundamental = NULL);
#endif

} /*namespace idl*/